/********************************************
* Copyrigt (C) rumi
* Module: serv_datapack.c
* History: Created on 2021.8.10
* Version:1.0.0 add ros2 support
* Version:1.0.1 fix comservice recive and send
* Version:1.0.2 change recive to stateMachine and adopt to geely NoDes
*********************************************/
#include "serv_datapack.h"
/********************************************/
static const uint32_t crc32_table[256] = {
    0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005,
    0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD,
    0x4C11DB70, 0x48D0C6C7, 0x4593E01E, 0x4152FDA9, 0x5F15ADAC, 0x5BD4B01B, 0x569796C2, 0x52568B75,
    0x6A1936C8, 0x6ED82B7F, 0x639B0DA6, 0x675A1011, 0x791D4014, 0x7DDC5DA3, 0x709F7B7A, 0x745E66CD,
    0x9823B6E0, 0x9CE2AB57, 0x91A18D8E, 0x95609039, 0x8B27C03C, 0x8FE6DD8B, 0x82A5FB52, 0x8664E6E5,
    0xBE2B5B58, 0xBAEA46EF, 0xB7A96036, 0xB3687D81, 0xAD2F2D84, 0xA9EE3033, 0xA4AD16EA, 0xA06C0B5D,
    0xD4326D90, 0xD0F37027, 0xDDB056FE, 0xD9714B49, 0xC7361B4C, 0xC3F706FB, 0xCEB42022, 0xCA753D95,
    0xF23A8028, 0xF6FB9D9F, 0xFBB8BB46, 0xFF79A6F1, 0xE13EF6F4, 0xE5FFEB43, 0xE8BCCD9A, 0xEC7DD02D,
    0x34867077, 0x30476DC0, 0x3D044B19, 0x39C556AE, 0x278206AB, 0x23431B1C, 0x2E003DC5, 0x2AC12072,
    0x128E9DCF, 0x164F8078, 0x1B0CA6A1, 0x1FCDBB16, 0x018AEB13, 0x054BF6A4, 0x0808D07D, 0x0CC9CDCA,
    0x7897AB07, 0x7C56B6B0, 0x71159069, 0x75D48DDE, 0x6B93DDDB, 0x6F52C06C, 0x6211E6B5, 0x66D0FB02,
    0x5E9F46BF, 0x5A5E5B08, 0x571D7DD1, 0x53DC6066, 0x4D9B3063, 0x495A2DD4, 0x44190B0D, 0x40D816BA,
    0xACA5C697, 0xA864DB20, 0xA527FDF9, 0xA1E6E04E, 0xBFA1B04B, 0xBB60ADFC, 0xB6238B25, 0xB2E29692,
    0x8AAD2B2F, 0x8E6C3698, 0x832F1041, 0x87EE0DF6, 0x99A95DF3, 0x9D684044, 0x902B669D, 0x94EA7B2A,
    0xE0B41DE7, 0xE4750050, 0xE9362689, 0xEDF73B3E, 0xF3B06B3B, 0xF771768C, 0xFA325055, 0xFEF34DE2,
    0xC6BCF05F, 0xC27DEDE8, 0xCF3ECB31, 0xCBFFD686, 0xD5B88683, 0xD1799B34, 0xDC3ABDED, 0xD8FBA05A,
    0x690CE0EE, 0x6DCDFD59, 0x608EDB80, 0x644FC637, 0x7A089632, 0x7EC98B85, 0x738AAD5C, 0x774BB0EB,
    0x4F040D56, 0x4BC510E1, 0x46863638, 0x42472B8F, 0x5C007B8A, 0x58C1663D, 0x558240E4, 0x51435D53,
    0x251D3B9E, 0x21DC2629, 0x2C9F00F0, 0x285E1D47, 0x36194D42, 0x32D850F5, 0x3F9B762C, 0x3B5A6B9B,
    0x0315D626, 0x07D4CB91, 0x0A97ED48, 0x0E56F0FF, 0x1011A0FA, 0x14D0BD4D, 0x19939B94, 0x1D528623,
    0xF12F560E, 0xF5EE4BB9, 0xF8AD6D60, 0xFC6C70D7, 0xE22B20D2, 0xE6EA3D65, 0xEBA91BBC, 0xEF68060B,
    0xD727BBB6, 0xD3E6A601, 0xDEA580D8, 0xDA649D6F, 0xC423CD6A, 0xC0E2D0DD, 0xCDA1F604, 0xC960EBB3,
    0xBD3E8D7E, 0xB9FF90C9, 0xB4BCB610, 0xB07DABA7, 0xAE3AFBA2, 0xAAFBE615, 0xA7B8C0CC, 0xA379DD7B,
    0x9B3660C6, 0x9FF77D71, 0x92B45BA8, 0x9675461F, 0x8832161A, 0x8CF30BAD, 0x81B02D74, 0x857130C3,
    0x5D8A9099, 0x594B8D2E, 0x5408ABF7, 0x50C9B640, 0x4E8EE645, 0x4A4FFBF2, 0x470CDD2B, 0x43CDC09C,
    0x7B827D21, 0x7F436096, 0x7200464F, 0x76C15BF8, 0x68860BFD, 0x6C47164A, 0x61043093, 0x65C52D24,
    0x119B4BE9, 0x155A565E, 0x18197087, 0x1CD86D30, 0x029F3D35, 0x065E2082, 0x0B1D065B, 0x0FDC1BEC,
    0x3793A651, 0x3352BBE6, 0x3E119D3F, 0x3AD08088, 0x2497D08D, 0x2056CD3A, 0x2D15EBE3, 0x29D4F654,
    0xC5A92679, 0xC1683BCE, 0xCC2B1D17, 0xC8EA00A0, 0xD6AD50A5, 0xD26C4D12, 0xDF2F6BCB, 0xDBEE767C,
    0xE3A1CBC1, 0xE760D676, 0xEA23F0AF, 0xEEE2ED18, 0xF0A5BD1D, 0xF464A0AA, 0xF9278673, 0xFDE69BC4,
    0x89B8FD09, 0x8D79E0BE, 0x803AC667, 0x84FBDBD0, 0x9ABC8BD5, 0x9E7D9662, 0x933EB0BB, 0x97FFAD0C,
    0xAFB010B1, 0xAB710D06, 0xA6322BDF, 0xA2F33668, 0xBCB4666D, 0xB8757BDA, 0xB5365D03, 0xB1F740B4,
};

uint32_t CRC32_Check_Code(uint8_t *data, uint32_t length)
{
    uint8_t index = 0u;
    uint32_t crc = 0xFFFFFFFFu;
    uint32_t i = 0u;

    if(length >= 256)
    {
        return 0xFFFFFFFFu; 
    }

    for(i = 0u; i < length; i++)
    {
        index = (crc >> 24) ^ data[i];
        crc = (crc << 8) ^ crc32_table[index];
    }
    return crc^ 0xFFFFFFFF;
}

uint16_t CRC16_Check_Code(uint8_t * data, uint16_t length)
{
    uint16_t i, crc_reg, CrcData;

    crc_reg = 0xFFFF;

    while(length--)
    {
        CrcData = *data++;
        crc_reg ^= CrcData;

        for(i = 0; i < 8; i++)
        {
            if(crc_reg & 0x01)
            {
                crc_reg = (crc_reg >> 1) ^ 0xA001;
            }
            else
            {
                crc_reg = (crc_reg >> 1);
            }
        }
    }
    return crc_reg;		
}

/************************************************************************
* CBuff_Init
************************************************************************/
void CBuff_Init(CBuff *buffer, uint8_t *pBuff, uint32_t size)
{
    buffer->pBuff = pBuff;
    buffer->Size = size;
    buffer->Head = 0;
    buffer->Tail = 0;
}

/************************************************************************
* CBuff_GetLength
************************************************************************/
uint32_t CBuff_GetLength(const CBuff *buffer)
{
    return (buffer->Head >= buffer->Tail) ? (buffer->Head - buffer->Tail) : (buffer->Size + buffer->Head - buffer->Tail);
}

/************************************************************************
* CBuff_Write
************************************************************************/
bool CBuff_Write(CBuff *buffer, const uint8_t *data, uint32_t len)
{
    uint32_t freeSpace = buffer->Size - CBuff_GetLength(buffer) - 1; 
    uint32_t Head,i;

    if (freeSpace < len)
    {
        return false; 
    }

    Head = buffer->Head;
    for (i = 0; i < len; i++)
    {
        buffer->pBuff[Head] = data[i];
        Head = (Head + 1) % buffer->Size;
    }
    buffer->Head = Head;

    return true; 
}
/************************************************************************
* CBuff_Read
************************************************************************/
bool CBuff_Read(CBuff *buffer, uint8_t *data, uint32_t len)
{
    uint32_t availableData = CBuff_GetLength(buffer);
    uint32_t Tail,i;

    if (availableData < len)
    {
        return false; 
    }
    
    Tail = buffer->Tail;
    for (i = 0; i < len; i++)
    {
        data[i] = buffer->pBuff[Tail];
		Tail = (Tail + 1) % buffer->Size;
    }
    
    return true; 
}
/************************************************************************
* CBuff_Pop
************************************************************************/
bool CBuff_Pop(CBuff *buffer, uint8_t *data, uint32_t len)
{
    uint32_t availableData = CBuff_GetLength(buffer);
    uint32_t Tail,i;

    if (availableData < len)
    {
        return false; 
    }
    
    Tail = buffer->Tail;
    for (i = 0; i < len; i++)
    {
        data[i] = buffer->pBuff[Tail];
        Tail = (Tail + 1) % buffer->Size;
    }
    buffer->Tail = Tail;
    return true; 
}

/************************************************************************
* CBuff_Clear
************************************************************************/
void CBuff_Clear(CBuff *buffer)
{
    buffer->Head = 0;
    buffer->Tail = 0;
}

/************************************************************************
* CBuff_IsEmpty
************************************************************************/
bool CBuff_IsEmpty(const CBuff *buffer)
{
    return (buffer->Head == buffer->Tail);
}

/************************************************************************
* CBuff_IsFull
************************************************************************/
bool CBuff_IsFull(const CBuff *buffer)
{
    return ((buffer->Head + 1) % buffer->Size) == buffer->Tail;
}

/************************************************************************
* CBuff_GetFreeSpace
************************************************************************/
uint32_t CBuff_GetFreeSpace(const CBuff *buffer)
{
    return buffer->Size - CBuff_GetLength(buffer) - 1; 
}


uint8_t* Serv_Com_PackageData(COM_FRAME_HEAD_ENUM FrameHead,COM_FRAME_CRC_ENUM FrameTail,uint8_t *Buffer,uint8_t DataLen)
{
    uint8_t lbOut = true;
    static uint8_t TmpSendBuff[64];
    uint8_t SendCount=0;
    COM_SEND_STATE_ENUM Send_State = E_COMSENSENDSTATE_INIT;
    while(lbOut)
    {
        switch(Send_State)
        {
            case E_COMSENSENDSTATE_INIT:
            {    
                SendCount = 0;
                Send_State = E_COMSENSENDSTAT_FRAMEHEAD;      
                break;
            }

            case E_COMSENSENDSTAT_FRAMEHEAD:
            {
                switch (FrameHead)
                {
                    case E_COMFRAMEHEAD_NONE:
                    {
                        TmpSendBuff[SendCount++]=DEVICE_HEAD;
                        TmpSendBuff[SendCount++]=0x00;
                        TmpSendBuff[SendCount++]=0; // Length Byte
                        break;
                    }

                    case E_COMFRAMEHEAD_NORMAL:
                    {
                        TmpSendBuff[SendCount++]=DEVICE_HEAD;
                        TmpSendBuff[SendCount++]=0xA0;
                        TmpSendBuff[SendCount++]=0; // Length Byte                       
                        break;
                    }
                    default:
                        break; 
                }
                Send_State = E_COMSENSENDSTAT_DATA;                          
                break;
            }
            
            case E_COMSENSENDSTAT_DATA:
            {
                for (uint8_t i = 0; i < DataLen; i++)
                {
                    TmpSendBuff[SendCount++] = Buffer[i];
                }
                Send_State = E_COMSENSENDSTAT_FRAMEEND;               
                break;
            }

            case E_COMSENSENDSTAT_FRAMEEND:
            {
                switch (FrameTail)
                {
                    case E_COMFRAMETAIL_NONE:
                    {
                        TmpSendBuff[1] += 0;
                        break;
                    }
                    case E_COMFRAMETAIL_JustFloat:
                    {
                        TmpSendBuff[1] += 1;
                        TmpSendBuff[SendCount++] = 0x00;
                        TmpSendBuff[SendCount++] = 0x00;
                        TmpSendBuff[SendCount++] = 0x80;
                        TmpSendBuff[SendCount++] = 0x7F;
                        break;
                    }
                    case E_COMFRAMETAIL_CRC16:
                    {
                        TmpSendBuff[1] += 5;
                        uint16_t u16CRC = CRC16_Check_Code(Buffer,DataLen);
                        TmpSendBuff[SendCount++] = (u16CRC >> 8) & 0xFF;
                        TmpSendBuff[SendCount++] = u16CRC & 0xFF;
                        break;
                    }                    
                    default:
                        break; 
                }
                Send_State = E_COMSENSENDSTAT_LENGTH;          
                break;
            }

            case E_COMSENSENDSTAT_LENGTH:
            {
                TmpSendBuff[PackDataLen] = SendCount;
                lbOut = false; 
                break;
            }

            default:
            {
                lbOut = false;
                break;
            }
        }
    }
    return TmpSendBuff;
}
/********************************************/
// run in 5ms handle
uint8_t Serv_Com_UnpackFSM(CBuff *lRecive_CBuff, uint8_t *sucReciveData)
{
    uint32_t CRC_Sum=0,CRC_Calc=0xFFFFFFFF;
    uint8_t ret=0;

    switch(lRecive_CBuff->fsm)
    {
        case E_COM_SYNC_SEARCH:
        {
            if(CBuff_GetLength(lRecive_CBuff) < 1){
                break;}// waiting for data
            else{
                CBuff_Pop(lRecive_CBuff,sucReciveData,1);
                if((sucReciveData[0] == 0xAA)||(sucReciveData[0] == 0x79)||(sucReciveData[0] == 0xBB)){               
                    lRecive_CBuff->fsm = E_COM_HEADER_PROCESS;
                    switch(sucReciveData[0])
                    {
                        case 0xAA:
                        case 0xBB:
                        {
                            lRecive_CBuff->HeadLen = 3;
                            lRecive_CBuff->DataMaxLen = 64;
                            lRecive_CBuff->Lenindex = 2;
                        }
                        case 0x79:
                        {
                            lRecive_CBuff->HeadLen = 5;
                            lRecive_CBuff->DataMaxLen = 120;                           
                            lRecive_CBuff->Lenindex = 4;
                        }
                        default:
                            break;
                    }
                }
                else{break;}
                
            }    
        }

        case E_COM_HEADER_PROCESS:
        {
            if(CBuff_GetLength(lRecive_CBuff) < lRecive_CBuff->HeadLen-1){
                break;}// waiting for data
            else{
                CBuff_Read(lRecive_CBuff,sucReciveData+1,lRecive_CBuff->HeadLen-1);
                lRecive_CBuff->HeadType = E_COMFRAMEHEAD_NONE;   
                lRecive_CBuff->TailType = E_COMFRAMETAIL_NONE;
                switch(sucReciveData[0])
                {
                    case 0xAA:
                    case 0xBB:
                    {
                        switch(sucReciveData[1])
                        {
                            case 0xA0:lRecive_CBuff->HeadType = E_COMFRAMEHEAD_NORMAL;lRecive_CBuff->TailType = E_COMFRAMETAIL_NONE; break;
                            case 0xA1:lRecive_CBuff->HeadType = E_COMFRAMEHEAD_NORMAL;lRecive_CBuff->TailType = E_COMFRAMETAIL_JustFloat;break;
                            case 0xA5:lRecive_CBuff->HeadType = E_COMFRAMEHEAD_NORMAL;lRecive_CBuff->TailType = E_COMFRAMETAIL_CRC16;break;           
                            case 0xB0:lRecive_CBuff->HeadType = E_COMFRAMEHEAD_UPDATE;lRecive_CBuff->TailType = E_COMFRAMETAIL_NONE; break;          
                            case 0xB5:lRecive_CBuff->HeadType = E_COMFRAMEHEAD_UPDATE;lRecive_CBuff->TailType = E_COMFRAMETAIL_CRC16;break;
                            default:break;
                        }
                        break;
                    }
                    case 0x79:
                    {
                        lRecive_CBuff->HeadType = E_COMFRAMEHEAD_NORMAL;
                        lRecive_CBuff->TailType = E_COMFRAMETAIL_CRC32;
                        break;
                    }
                    default:break;
                }
                
                switch(lRecive_CBuff->TailType)
                {
                    case E_COMFRAMETAIL_NONE:
                    case E_COMFRAMETAIL_CRC16:
                    case E_COMFRAMETAIL_CRC32:
                    {
                        lRecive_CBuff->DataLen = sucReciveData[lRecive_CBuff->Lenindex];
                        break;
                    }
                }

                if(sucReciveData[lRecive_CBuff->Lenindex] > lRecive_CBuff->DataMaxLen){
                    lRecive_CBuff->fsm = E_COM_SYNC_SEARCH;}
                else{
                    lRecive_CBuff->fsm = E_COM_DATA_PROCESS;}
            }
        }

        case E_COM_DATA_PROCESS:
        {
            if(CBuff_GetLength(lRecive_CBuff) < lRecive_CBuff->HeadLen+lRecive_CBuff->DataLen-1){
                break;  // waiting for data
            }else{
                CBuff_Read(lRecive_CBuff,sucReciveData+1,lRecive_CBuff->DataLen+lRecive_CBuff->HeadLen-1);
            }
            switch(lRecive_CBuff->TailType)
            {
                case E_COMFRAMETAIL_NONE:
                {
                    CRC_Calc = 0xfefefefe;
                    CRC_Sum = 0xfefefefe;
                    break;
                }
                case E_COMFRAMETAIL_CRC16:
                {
                    CRC_Calc = CRC16_Check_Code(sucReciveData+lRecive_CBuff->HeadLen,(lRecive_CBuff->DataLen-2)); 
                    CRC_Calc &= 0x0000FFFF;
                    CRC_Sum = sucReciveData[lRecive_CBuff->HeadLen+lRecive_CBuff->DataLen-2]*256 
                            + sucReciveData[lRecive_CBuff->HeadLen+lRecive_CBuff->DataLen-1];
                    CRC_Sum &= 0x0000FFFF;
                    break;
                }

                case E_COMFRAMETAIL_CRC32:
                {
                    if(lRecive_CBuff->DataLen <= 4)
                    {
                        CRC_Calc = 0xFFFFFFFFu;
                    }
                    else
                    {
                        CRC_Calc = CRC32_Check_Code(sucReciveData+lRecive_CBuff->HeadLen,(lRecive_CBuff->DataLen-4));
                    }                  
                    CRC_Sum = (sucReciveData[lRecive_CBuff->HeadLen+lRecive_CBuff->DataLen-4] << 24)
                            | (sucReciveData[lRecive_CBuff->HeadLen+lRecive_CBuff->DataLen-3] << 16)
                            | (sucReciveData[lRecive_CBuff->HeadLen+lRecive_CBuff->DataLen-2] << 8)
                            | (sucReciveData[lRecive_CBuff->HeadLen+lRecive_CBuff->DataLen-1]);
                    break;
                }
                default:break;
            }
            	
            if(CRC_Calc != CRC_Sum)
            {
                lRecive_CBuff->fsm = E_COM_SYNC_SEARCH;
                break;
            }
            else
            {
                CBuff_Pop(lRecive_CBuff,sucReciveData+1,lRecive_CBuff->HeadLen+lRecive_CBuff->DataLen-1);
                ret=1;                             
            }

            lRecive_CBuff->fsm = E_COM_SYNC_SEARCH; 
            break;
            
        }
        default:break;
    }
    return ret;
}
/********************************************
 * End of file
*********************************************/
